package org.robotfish.geometry;

import org.ejml.simple.SimpleMatrix;
import org.robotfish.utils.SVGDocument;
import org.w3c.dom.Comment;
import org.w3c.dom.Element;

/**
 * Lowest level Transformation.
 * This class represents a arc and a ray. Points from the ray are transformed to arc. 
 * @author Mart Anton
 */
public class TransformationArc extends Transformation {
	
	/**
	 * The start of the arc.
	 */
	SimpleMatrix ray;
	
	
	double curvature;
	
	/**
	 * Derivative of the natural parameter of the curve with respect to the parameter. Default value is 1.
	 */
	double ds;
	
	/**
	 * The last element of 
	 * Because we want to do interpolation, the index of the last element will not be given
	 */
//	public static ArcTransformation END = new ArcTransformation(Double.POSITIVE_INFINITY, new SimpleMatrix(3,1), 0, 1);

	/**
	 * Creates arc section of the curve
	 * @param start the value of the curve parameter at the start of the arc
	 * @param ray start point and direction of the arc {x,y,direction}
	 * @param curvature curvature of the arc
	 * @param ds Derivative of the natural parameter of the curve with respect to the parameter.
	 */
	public TransformationArc(double start, SimpleMatrix ray, double ds, double curvature) {
		this.coordinateValue = start;
		this.ray = ray;
		this.curvature = curvature;
		this.ds = ds;
	}

	/**
	 * Creates arc section of the curve, with natural parameter as parameter
	 * @param start the value of the curve parameter at the start of the arc
	 * @param ray start point and direction of the arc {x,y,direction}
	 * @param curvature curvature of the arc
	 */
	public TransformationArc(double start, SimpleMatrix ray, double curvature) {
		this(start, ray, curvature, 1);
	}
	
	
/*	public ArcTransformation getParallelArc(double Dy, double newStart, double newDs) {
		ArcTransformation result = getParallelLine(angle, dinstance, newStart, newDs);
		result.curvature = -1/Dy;
		return result;
	}*/

	public double getCurvature() {
		return curvature;
	}

	public double getDs() {
		return ds;
	}
	
	@Override
	public int getCoordinate() {
		return 0;
	}
	
	/**
	 * Get ray at distance of a point specified by parameter
	 * @param p parameter of the curve
	 * @param h distance from the curve
	 * @return {{x,y}, direction}
	 */
	public SimpleMatrix getRay(double p, double h) {
		SimpleMatrix newRay = getRay(p);
		return newRay.plus(normalVector(newRay.get(2)).scale(h));
	}
	
	/**
	 * Get ray at point specified by parameter
	 * @param p parameter of the curve
	 * @return {{x,y}, direction}
	 */
	public SimpleMatrix getRay(double p) {
		double Ds = (p-coordinateValue)*ds;
		double Da05 = Ds*curvature*0.5;
		Ds = Ds*sinc(Da05);
		SimpleMatrix newRay = tangentVector(ray.get(2)+Da05);
		newRay = newRay.scale(Ds);
		newRay.set(2, Da05+Da05);
		return newRay.plus(ray);
	}
	
	@Override
	public SimpleMatrix getRay() {
		return ray;
	}
	
	
	public int getColorIndex() {
		return 0;
	}
	
	public String toString() {
		return "Arc(ray{point("+ray.get(0)+", "+ray.get(1)+"), "+ray.get(2)+"}, k{"+curvature+"}, dp{"+ds+"})";
	}

	@Override
	public SimpleMatrix transform(SimpleMatrix point) {
		SimpleMatrix result;
		result = getRay(point.get(0));
	
		SimpleMatrix trnPoint = point.copy();
		trnPoint.set(0, result.get(0));
		trnPoint.set(1, result.get(1));
		return trnPoint; 
	}
	
	@Override
	public SimpleMatrix interpolate(SimpleMatrix point, Transformation X1, double m0) {
		// kaare puhul tegelikult interpoleerimist ei toimu
		if (point.get(2)>= X1.coordinateValue)
			return X1.transform(point);
		return transform(point);
//		throw new RuntimeException("ArcTransformation does not support intrpolation!");
	}

	@Override
	public void appendSVGElements(SVGDocument doc, Element parent, double arcLength) {
		
		Comment comment = doc.createComment(getClass().getName()+".appendSVGElements: curvature="+curvature+", ds="+ds);
        parent.appendChild(comment);
        
        SimpleMatrix endRay = getRay(coordinateValue+arcLength);
        
        if (Math.abs(curvature)<1E-6) // kõverust on liiga vähe
        	parent.appendChild(doc.createLine(ray, endRay, doc.getColour(getColorIndex())));
        else
        	parent.appendChild(doc.createArc(ray, endRay, curvature, doc.getColour(getColorIndex())));

        SimpleMatrix midRay = getRay(coordinateValue+arcLength*0.5);
        
        String startValueS = SVGDocument.DoubleToString3p(coordinateValue);
        String endValueS = SVGDocument.DoubleToString3p(coordinateValue+arcLength);
        if (!startValueS.equals(endValueS))
        	parent.appendChild(doc.createText(midRay, "x in ["+startValueS+","+endValueS+")", doc.getColour(getColorIndex()+4)));
        
	}
	
	
}
